#include "Pose3D.h"
#include<cmath>


void Pose3D::fromROT4()
{
	const double cy = cos(yaw);
	const double sy = sin(yaw);
	const double cp = cos(pitch);
	const double sp = sin(pitch);
	const double cr = cos(roll);
	const double sr = sin(roll);

	m_ROT4 << cy * cp, cy * sp * sr - sy * cr, cy * sp * cr + sy * sr, x,
		sy * cp, sy * sp * sr + cy * cr, sy * sp * cr - cy * sr, y,
		-sp, cp * sr, cp * cr, z,
		0, 0, 0, 1;
}

void Pose3D::fromROT3(double _yaw, double _pitch, double _roll)
{
	const double cy = cos(_yaw);
	const double sy = sin(_yaw);
	const double cp = cos(_pitch);
	const double sp = sin(_pitch);
	const double cr = cos(_roll);
	const double sr = sin(_roll);

	m_ROT3 << cy * cp, cy * sp * sr - sy * cr, cy * sp * cr + sy * sr,
		sy * cp, sy * sp * sr + cy * cr, sy * sp * cr - cy * sr,
		-sp, cp * sr, cp * cr;
}

void Pose3D::fromROT3()
{
	const double cy = cos(yaw);
	const double sy = sin(yaw);
	const double cp = cos(pitch);
	const double sp = sin(pitch);
	const double cr = cos(roll);
	const double sr = sin(roll);
	m_ROT3 << cy * cp, cy * sp * sr - sy * cr, cy * sp * cr + sy * sr,
		sy * cp, sy * sp * sr + cy * cr, sy * sp * cr - cy * sr,
		-sp, cp * sr, cp * cr;
}

void Pose3D::fromROT4(double _x, double _y, double _z, double _yaw, double _pitch, double _roll)
{
	const double cy = cos(_yaw);
	const double sy = sin(_yaw);
	const double cp = cos(_pitch);
	const double sp = sin(_pitch);
	const double cr = cos(_roll);
	const double sr = sin(_roll);

	m_ROT4 << cy * cp, cy * sp * sr - sy * cr, cy * sp * cr + sy * sr, _x,
		sy * cp, sy * sp * sr + cy * cr, sy * sp * cr - cy * sr, _y,
		-sp, cp * sr, cp * cr, _z,
		0,     0,     0,     1;
}

void Pose3D::transformPoint(const double & lx, const double & ly, const double & lz, float& gx, float& gy, float& gz) const
{
	gx = m_ROT3(0, 0) * lx + m_ROT3(0, 1) * ly + m_ROT3(0, 2) * lz + x;
	gy = m_ROT3(1, 0) * lx + m_ROT3(1, 1) * ly + m_ROT3(1, 2) * lz + y;
	gz = m_ROT3(2, 0) * lx + m_ROT3(2, 1) * ly + m_ROT3(2, 2) * lz + z;
}

double Pose3D::distance2Pose(const float & px, const float & py, const float & pz) const
{
	return std::sqrt(std::pow(x - px, 2) + std::pow(y - py, 2) + std::pow(z - pz, 2));
}

double Pose3D::distance2Pose(const Pose3D & pose) const
{
	//TODO
	return 0;
}

Pose3D::Pose3D()
{
	m_ROT3 = Eigen::Matrix3d::Identity();
	m_ROT4 = Eigen::Matrix4d::Identity();
}


Pose3D::~Pose3D()
{
}
